#include "CamCal.h"
#include <iostream>
#include <cstdlib>
#include <fstream>
using namespace std;

void CamCal::init(int x, int y)
{
	centerx = x;
	centery = y;
}

bool CamCal::read(const char* filename){
	double a,b,c;
	ifstream fin(filename);
	bool flag = false;
	int i=-1,j=0;
	try{
		while(fin>>a>>b>>c)
		{
			point p;
			p.px = a;
			p.py = b;
			p.r = c;
			data.push_back(p);
		}
		return true;}
	catch(...){	return false;}
}

bool CamCal::write(const char* filename){
	ofstream fout(filename);
	for (int i=0;i<col;i++){
		fout<<cvGetReal2D(Cx,i,0)<<" ";
		cout<<"coe "<<i<<": "<<cvGetReal2D(Cx,i,0)<<endl;
	}
	return true;
}

void CamCal::compute(){
	row=data.size();
	col = 3;
	CA =  cvCreateMat(row,col,CV_32FC1);
	Cb =  cvCreateMat(row,1,CV_32FC1);
	Cx =  cvCreateMat(col,1,CV_32FC1);

	for (int i=0;i<row;i++){
		double x = data[i].px;
		double y = data[i].py;
		double r = data[i].r;
		double tmp[3];
		x = sqrt((x-centerx)*(x-centerx)+(y-centery)*(y-centery));
		tmp[0] = 1;			tmp[1] = x;		tmp[2] = x*x;
		for (int j=0;j<col;j++){
			cvSetReal2D(CA,i,j,tmp[j]);
		}
		cvSetReal2D(Cb,i,0,r);
	}
	cvSolve(CA,Cb,Cx,CV_SVD);
}

double CamCal::pixToDistance(double x,double y){
	double tmp[20];
	double r=0;
	x = sqrt((x-centerx)*(x-centerx)+(y-centery)*(y-centery));
	tmp[0] = 1;			tmp[1] = x;		tmp[2] = x*x;		
	for(int i=0;i<col;i++)
		r += tmp[i]*cvGetReal2D(Cx,i,0);
	return abs(r); 
}

void CamCal::error(){
	for(int i=0;i<row;i++){
		double Evalue = pixToDistance(data[i].px,data[i].py);
		cout<<i<<" X: "<<data[i].px<<" Y: "<<data[i].py<<" real: "<<data[i].r<<" evaluate: "<<Evalue<<" err"<<": "<<abs(Evalue-data[i].r)<<endl;
	}
}

void main(int argc, char **argv)
{
	CamCal c;
	c.init(atoi(argv[1]), atoi(argv[2]));
	c.read(argv[3]);
	c.compute();
	c.write(argv[4]);
	c.error();
}

